{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "using Interact\n",
    "using RigidBodyDynamics\n",
    "using RigidBodyTreeInspector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Open the viewer application\n",
    "DrakeVisualizer.any_open_windows() || DrakeVisualizer.new_window()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Construct a mechanism\n",
    "mechanism = rand_chain_mechanism(Float64, [QuaternionFloating{Float64}; [Revolute{Float64} for i = 1:5]]...)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Construct the visualizer interface, and get the :robot1 subtree\n",
    "vis = DrakeVisualizer.Visualizer()[:robot1];"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Load the mechanism's geometry into the visualizer\n",
    "setgeometry!(vis, mechanism)\n",
    "\n",
    "# We can draw the mechanism at a single state:\n",
    "state = MechanismState{Float64}(mechanism)\n",
    "settransform!(vis, state);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Or we can interactively explore the degrees of freedom of the mechanism.\n",
    "# Note that the quaternion floating base joint, \"joint1\", has been given \n",
    "# six sliders. The first three correspond to the exponential map representation\n",
    "# of its rotation, and the next three correspond to its translation. \n",
    "inspect(mechanism; show_inertias=true, randomize_colors=true);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We can visualize reference frames and points fixed to bodies:\n",
    "frame = frame_after(last(tree_joints(mechanism)))\n",
    "point = Point3D(frame, 0.1, 0.2, 0.3)\n",
    "addgeometry!(vis, mechanism, frame; scale=0.5)\n",
    "addgeometry!(vis, mechanism, point; radius=0.05)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# We can also animate the mechanism, given a time sequence of states.\n",
    "# States will be linearly interpolated between the knot points.\n",
    "times = collect(linspace(0, 10, 3))\n",
    "configurations = Vector{Vector{Float64}}(length(times))\n",
    "for i = 1:length(times)\n",
    "    rand_configuration!(state)\n",
    "    configurations[i] = copy(configuration(state))\n",
    "end\n",
    "animate(vis, mechanism, times, configurations; fps = 60., realtimerate = 1.)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# We can simulate a mechanism from an initial state. We'll use a mechanism\n",
    "# without a QuaternionFloating joint so that it doesn't just fall straight down.\n",
    "mechanism = rand_chain_mechanism(Float64, [Revolute{Float64} for i = 1:10]...)\n",
    "vis = Visualizer()[:robot2]\n",
    "setgeometry!(vis, mechanism)\n",
    "state = MechanismState{Float64}(mechanism)\n",
    "settransform!(vis, state)\n",
    "times, configurations, velocities = simulate(state, 10.; Δt = 0.001);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# And we can animate the result in realtime\n",
    "# (note that it looks a little strange because the link center of mass locations aren't where you think they are)\n",
    "animate(vis, mechanism, times, configurations)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We can also inspect individual frames from the simulation\n",
    "@manipulate for i in 1:length(times)\n",
    "    set_configuration!(state, configurations[i])\n",
    "    set_velocity!(state, velocities[i])\n",
    "    settransform!(vis, state)\n",
    "    times[i]\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 3D visualization can also be done *while simulating*, using DrakeVisualizerSink in combination with\n",
    "# the lower level RigidBodyDynamics ODE integration functionality:\n",
    "using RigidBodyDynamics.OdeIntegrators\n",
    "result = DynamicsResult{Float64}(mechanism)\n",
    "function damped_dynamics!(vd::AbstractArray, sd::AbstractArray, t, state)\n",
    "    damping = 2.\n",
    "    τ = -damping * velocity(state)\n",
    "    dynamics!(result, state, τ)\n",
    "    copy!(vd, result.v̇)\n",
    "    copy!(sd, result.ṡ)\n",
    "    nothing\n",
    "end\n",
    "integrator = MuntheKaasIntegrator(damped_dynamics!, runge_kutta_4(Float64), DrakeVisualizerSink(vis))\n",
    "integrate(integrator, state, 10., 1e-3, max_realtime_rate = 1.)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Julia 0.6.0",
   "language": "julia",
   "name": "julia-0.6"
  },
  "language_info": {
   "file_extension": ".jl",
   "mimetype": "application/julia",
   "name": "julia",
   "version": "0.6.0"
  },
  "widgets": {
   "state": {
    "08bade03-3d14-414a-8408-ed74f2e8f173": {
     "views": [
      {
       "cell_index": 5
      }
     ]
    },
    "0d760918-e55a-498b-adde-2f5c25701138": {
     "views": [
      {
       "cell_index": 5
      }
     ]
    },
    "162978fd-2971-4068-ade3-f5460f0ea3d6": {
     "views": [
      {
       "cell_index": 10
      }
     ]
    },
    "298c0cdd-c49e-4d30-b94c-c3d0f9883fbd": {
     "views": [
      {
       "cell_index": 5
      }
     ]
    },
    "5fc72b42-1891-485b-a93f-9f605d93e050": {
     "views": [
      {
       "cell_index": 5
      }
     ]
    },
    "6f627260-88fb-4be7-9cb3-034d979eccb7": {
     "views": [
      {
       "cell_index": 5
      }
     ]
    },
    "787e602c-75c8-4b4b-9742-1e103a41433c": {
     "views": [
      {
       "cell_index": 5
      }
     ]
    },
    "8487288e-457b-4456-a1a3-840ac184bb06": {
     "views": [
      {
       "cell_index": 5
      }
     ]
    },
    "9cdb5607-9b0d-4427-a05f-be9090c4dd19": {
     "views": [
      {
       "cell_index": 5
      }
     ]
    },
    "a0a9386b-100b-495b-8783-f4b06e8e0f65": {
     "views": [
      {
       "cell_index": 5
      }
     ]
    },
    "b605625c-e83a-48cf-9013-c12a27e85eeb": {
     "views": [
      {
       "cell_index": 5
      }
     ]
    },
    "ca79e62d-302f-4c60-ac95-e9053b9ce2a7": {
     "views": [
      {
       "cell_index": 5
      }
     ]
    }
   },
   "version": "1.2.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
